/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief  服务模块的主运行逻辑模块文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#ifndef COSTMAP_2D_COSTMAP_2D_ROS_H_
#define COSTMAP_2D_COSTMAP_2D_ROS_H_

#include <config_file.h>
#include <costmap_2d/costmap_2d_publisher.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/data_collector.h>
#include <costmap_2d/footprint.h>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/msg_struct.h>
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/static_layer.h>
#include <time.h>

#include <vector>
namespace costmap_2d {
/**
 *@brief 地图模块主逻辑类
 *@author tuyinan
 *@version 2022-04-xx
 */
class Costmap2DROS {
 public:
  /**
   *@brief 初始化函数
   *@param const std::string& name 模块名
   *@param std::string config 配置参数地址
   *@param DataCollector* collector 数据获取类
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  Costmap2DROS(const std::string& name, std::string config,
               DataCollector* collector);
  /**
   *@brief 析构函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  ~Costmap2DROS();
  /**
   *@brief 开始更新的方法
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void start();
  /**
   *@brief 停止更新的方法
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void stop();
  /**
   *@brief 更新地图
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void updateMap();
  /**
   *@brief 重置地图
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void resetLayers();
  /**
   *@brief 判断各层时间是否为现在
   *@param 无
   *@return bool true 为是，false为否
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  bool isCurrent() { return layered_costmap_->isCurrent(); }
  /**
   *@brief 获取代价地图名
   *@param 无
   *@return string 地图名
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  std::string getName() const { return name_; }
  /**
   *@brief 返回代价地图
   *@param 无
   *@return Costmap2D* 代价地图
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  Costmap2D* getCostmap() { return layered_costmap_->getCostmap(); }
  /**
   *@brief 返回代价地图
   *@param 无
   *@return LayeredCostmap* 代价地图
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  LayeredCostmap* getLayeredCostmap() { return layered_costmap_; }
  /**
   *@brief 返回机器人占位
   *@param 无
   *@return costmap_2d::Polygon 占位
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  costmap_2d::Polygon getRobotFootprintPolygon() {
    return costmap_2d::toPolygon(padded_footprint_);
  }
  /**
   *@brief 返回机器人占位
   *@param 无
   *@return std::vector<costmap_2d::Point> 占位
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  std::vector<costmap_2d::Point> getRobotFootprint() {
    return padded_footprint_;
  }
  /**
   *@brief 返回机器人未膨胀的占位
   *@param 无
   *@return std::vector<costmap_2d::Point> 未膨胀的占位
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  std::vector<costmap_2d::Point> getUnpaddedRobotFootprint() {
    return unpadded_footprint_;
  }
  /**
   *@brief 设置机器人占位
   *@param const std::vector<costmap_2d::Point>& points 占位
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void setUnpaddedRobotFootprint(const std::vector<costmap_2d::Point>& points);

 protected:
  /// 代价地图
  LayeredCostmap* layered_costmap_;
  ///名字
  std::string name_;
  /**
   *@brief 地图更新线程
   *@param double frequency 更新频率
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void mapUpdateLoop(double frequency);
  /**
   *@brief 判断机器人是否移动的线程
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void movementCheckLoop();
  /// 地图更新线程停止符号
  bool map_update_thread_shutdown_;
  /// 更新，程序，机器人的停止符号
  bool stop_updates_, stopped_, robot_stopped_;
  /// 地图更新线程
  boost::thread* map_update_thread_;
  /// 移动确认线程
  boost::thread* movement_check_thread_;

  /// 上次发布地图的时间
  double last_publish_;
  /// 发布间隔时间
  double publish_cycle;
  /// 上次机器人位置
  costmap_2d::Pose old_pose_;
  /// 发布类
  Costmap2DPublisher* publisher_;
  /// 未膨胀占位
  std::vector<costmap_2d::Point> unpadded_footprint_;
  /// 膨胀后的占位
  std::vector<costmap_2d::Point> padded_footprint_;
  /// 占位膨胀半径
  float footprint_padding_;

  /// 数据获取类
  DataCollector* dataCollector_;
  /// 配置获取类
  ConfigFile* configNode_;

  /// 层级合集
  std::vector<boost::shared_ptr<costmap_2d::Layer>> layers_;
};
// class Costmap2DROS
}  // namespace costmap_2d

#endif  // COSTMAP_2D_COSTMAP_2D_ROS_H
